/**
 * @author Yunfeng Bai
 * 
 * 2012-7-18
 */
package simulation;

import geometry.Triangle;

import java.util.ArrayList;
import java.util.Arrays;
import java.util.HashSet;
import java.util.List;

import javax.vecmath.Vector3d;

import rigidbody.RigidBody;
import collision.Collision;
import collision.OBBTree;

public class ContactGraph
{
	List<RigidBody>[] graph;
	List<RigidBody>[] reverseGraph;
	List<RigidBody> bodies;
	RigidBody[] orderedRigidBodies;
	RigidBody[] indexMap;
	int[] timestamp;
	int[] stampedBodies;
	boolean[] visited;
	int currentTimestamp;
	int currentComponent;
	int bodyCount;
	double timestep;
	
	@SuppressWarnings("unchecked")
	public void init(List<RigidBody> bodies, double timestep) throws Exception
	{
		bodyCount = bodies.size();
		
		// Integral checks to make sure every RigidBody is using an unique id from 0 ~ bodyCount - 1.
		boolean usedId[] = new boolean[bodyCount];
		for (RigidBody body : bodies)
		{
			int id = body.getId();
			if (id >= bodyCount) throw new Exception("RigidBody id too big!");
			if (usedId[id]) throw new Exception("Two RigidBody are using the same ID!");
			usedId[id] = true;
		}
		
		graph = (List<RigidBody>[]) new ArrayList[bodyCount];
		reverseGraph = (List<RigidBody>[]) new ArrayList[bodyCount];
		indexMap = new RigidBody[bodyCount];
		for (int i = 0; i < bodyCount; i++)
		{
			graph[i] = new ArrayList<RigidBody>();
			reverseGraph[i] = new ArrayList<RigidBody>();
			indexMap[bodies.get(i).getId()] = bodies.get(i);
		}
		orderedRigidBodies = new RigidBody[bodyCount];
		timestamp = new int[bodyCount];
		stampedBodies = new int[bodyCount];
		visited = new boolean[bodyCount];
		currentTimestamp = 0;
		this.bodies = bodies;
		this.timestep = timestep;
	}
	
	/**
	 * For debug only.
	 */
	public List<RigidBody>[] getGraph()
	{
		return graph;
	}
	
	public List<RigidBody> getOrderedRigidBodies()
	{
//		List<RigidBody> yBodies = new ArrayList<RigidBody>();
//		for (RigidBody b : bodies)
//		{
//			yBodies.add(b);
//		}
//		for (int i = 0; i < yBodies.size(); i++)
//		{
//			for (int j = 0; j < i; j++)
//			{
//				if (yBodies.get(i).getPosition().y < yBodies.get(j).getPosition().y)
//				{
//					RigidBody temp = yBodies.get(i);
//					yBodies.set(i, yBodies.get(j));
//					yBodies.set(j, temp);
//				}
//			}
//		}
//		return yBodies;
		return Arrays.asList(orderedRigidBodies);
	}
	
	public void updateOrder()
	{
		// Reverse the graph.
		for (int i = 0; i < bodyCount; i++)
			reverseGraph[i].clear();
		
		for (int i = 0; i < bodyCount; i++)
		{
			RigidBody b1 = bodies.get(i);
			for (RigidBody b2 : graph[b1.getId()])
			{
				reverseGraph[b2.getId()].add(b1);
			}
		}
		
		updateComponents();
	}
	
	private void searchToGetTimestamps(int id)
	{
		visited[id] = true;
		for (RigidBody body : graph[id])
		{
			int bid = body.getId();
			if (!visited[bid])
			{
				searchToGetTimestamps(bid);
			}
		}
		
		timestamp[id] = currentTimestamp;
		stampedBodies[currentTimestamp] = id;
		currentTimestamp++;
	}
	
	private void updateComponents()
	{
		for (int i = 0; i < bodyCount; i++)
		{
			timestamp[i] = -1;
			visited[i] = false;
		}
		currentTimestamp = 0;
		for (int i = 0; i < bodyCount; i++)
		{
			int id = bodies.get(i).getId();
			if (timestamp[id] == -1)
				searchToGetTimestamps(id);
		}
		
		for (int i = 0; i < bodyCount; i++)
			visited[i] = false;
		currentComponent = bodyCount - 1;
		for (int i = bodyCount - 1; i >= 0; i--)
		{
			searchToGetOrder(stampedBodies[i]);
		}
	}

	public void updateGraph()
	{
		// Get the graph by temporarily move the objects there.
		HashSet<Triangle> triangles1 = new HashSet<Triangle>();
		HashSet<Triangle> triangles2 = new HashSet<Triangle>();
		
		for (int i = 0; i < bodies.size(); i++)
		{
			RigidBody body = bodies.get(i);
			if (body.isImmobile()) continue; // Immobile bodies are always at the lowest level of the graph.
			
			int id = body.getId();
			graph[id].clear();
			 	
			// Temporarily move b1.
			RigidBody b1 = body;
			b1.backupPosition();
			b1.integratePosition(timestep);
			
			for (int j = 0; j < bodies.size(); j++)
			{
				if (i == j) continue;
				
				RigidBody b2 = bodies.get(j);
				
				List<Collision> collisions = RigidBody.getCollisionsBetween(b1, b2, true);
				if (collisions.size() > 0)
				{
					graph[id].add(b2);
				}
			}
			
			b1.restorePosition();
		}
	}
	
	private void searchToGetOrder(int id)
	{
		if (visited[id]) return;
		visited[id] = true;
		orderedRigidBodies[currentComponent--] = indexMap[id];
		
		for (RigidBody body : reverseGraph[id])
		{
			int bid = body.getId();
			if (!visited[bid])
				searchToGetOrder(bid);
		}
	}
}
